#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

/**
 * @brief Initialize the param of the calculating of out param.
 * @param Param2D
 * @param Param3D
 * @param given_points_2D_mat
 * @param given_points_3D_mat
 */
void Initialize_Out_Param_Calculate(std::vector<cv::Point2f> *Param2D, std::vector<cv::Point3f> *Param3D, cv::Mat *given_points_2D_mat, cv::Mat *given_points_3D_mat){
    for(unsigned int i=0; i<40; i++){
        cv::Point2f temp_2D;
        cv::Point3f temp_3D;
        temp_2D.x = given_points_2D_mat->at<float>(i, 0);
        temp_2D.y = given_points_2D_mat->at<float>(i, 1);
        temp_3D.x = given_points_3D_mat->at<float>(i, 0);
        temp_3D.y = given_points_3D_mat->at<float>(i, 1);
        temp_3D.z = 0;
        Param2D->push_back(temp_2D);
        Param3D->push_back(temp_3D);
    }
}

/**
 * @brief Initialize the param of draw process.
 * @param unknown_points_3D
 * @param draw_obj
 */
void Initialize_Draw_Result(cv::Mat *unknown_points_3D, std::vector<cv::Point3f> *draw_obj){
    for(unsigned int i=0; i<36; i++){
        cv::Point3f temp_3D;
        temp_3D.x = unknown_points_3D->at<float>(i, 0);
        temp_3D.y = unknown_points_3D->at<float>(i, 1);
        temp_3D.z = 0;
        draw_obj->push_back(temp_3D);
    }
}

/**
 * @brief Draw and show the result.
 * @param draw_point
 * @param dst_img_clone
 */
void draw_result(std::vector<cv::Point2f> *draw_point, cv::Mat dst_img_clone){
    for(unsigned int i=0; i<(draw_point->size()+1)/4; i++){
        unsigned int k = i*4;
        cv::line(dst_img_clone, draw_point->at(k), draw_point->at(k+1), cv::Scalar(0, 255, 0), 2);
        cv::line(dst_img_clone, draw_point->at(k+1), draw_point->at(k+2), cv::Scalar(0, 255, 0), 2);
        cv::line(dst_img_clone, draw_point->at(k+2), draw_point->at(k+3), cv::Scalar(0, 255, 0), 2);
        cv::line(dst_img_clone, draw_point->at(k+3), draw_point->at(k), cv::Scalar(0, 255, 0), 2);
    }
    cv::imwrite("picture/dst_img.jpg", dst_img_clone);
    cv::imshow("dst", dst_img_clone);
    cv::waitKey();
}

/**
 * @brief The main process of the programe.
 * @return The state of the programe.
 * @author
 * -1160300719 殷浩然
 */
int main() {

    //自定义变量
    std::vector<cv::Point2f> Param2D;
    std::vector<cv::Point3f> Param3D;
    cv::Mat src_img = cv::imread("../picture/src_img.jpg");
    cv::Mat dst_img, dst_img_clone;
    cv::Mat camera_matrix, distort, zero_distort;
    cv::Mat rvec, tvec, Rotate;
    cv::FileStorage read_camera("../yml/camera.yml", cv::FileStorage::READ);
    cv::FileStorage read_given_points_2D("../yml/ten_quads_img_points.yml", cv::FileStorage::READ);
    cv::FileStorage read_given_points_3D("../yml/Given_Points.yml", cv::FileStorage::READ);
    cv::FileStorage read_unknown_points_3D("../yml/Unknown_Points.yml", cv::FileStorage::READ);

    //测试文件是否打开
    assert(read_camera.isOpened());
    assert(read_given_points_2D.isOpened());
    assert(read_given_points_3D.isOpened());
    assert(read_unknown_points_3D.isOpened());

    //Initialize_Out_Param_Calculate
    cv::Mat given_points_2D_mat = cv::Mat::eye(40, 2, CV_64F);
    cv::Mat given_points_3D_mat = cv::Mat::eye(40, 3, CV_64F);
    read_given_points_2D["ten_quads_img_points"] >> given_points_2D_mat;
    read_given_points_3D["given_points"] >> given_points_3D_mat;
    Initialize_Out_Param_Calculate(&Param2D, &Param3D, &given_points_2D_mat, &given_points_3D_mat);

    //去畸变
    read_camera["camera_matrix"] >> camera_matrix;
    read_camera["distortion_coefficients"] >> distort;
    cv::Mat R, new_Matrix, MAPX, MAPY;
    cv::initUndistortRectifyMap(camera_matrix, distort, R, new_Matrix, cv::Size(1920,1080), CV_32FC1, MAPX, MAPY);
    cv::remap(src_img, dst_img, MAPX, MAPY, cv::INTER_LINEAR);
    dst_img_clone = dst_img.clone();

    //求外参
    zero_distort = cv::Mat::eye(5, 1, CV_32F);
    for(int i=0; i<5; i++){
        zero_distort.at<float>(i, 0) = 0;
    }
    cv::solvePnP(Param3D, Param2D, camera_matrix, zero_distort, rvec, tvec, false, cv::SOLVEPNP_ITERATIVE);
    cv::Rodrigues(rvec, Rotate);

    //Initialize_Draw_Result
    std::vector<cv::Point2f> draw_point;
    std::vector<cv::Point3f> draw_obj;
    cv::Mat unknown_points_3D;
    read_unknown_points_3D["unknown_points"] >> unknown_points_3D;
    Initialize_Draw_Result(&unknown_points_3D, &draw_obj);

    //Show result
    cv::projectPoints(draw_obj, rvec, tvec, camera_matrix, zero_distort, draw_point);
    draw_result(&draw_point, dst_img_clone);

}